#include "main.h"
#include "printf.h"
#include "foc.h"
#include "mt6835.h"
#include "adc.h"
#include "hardware.h"
#include <math.h>

// 中断服务程序中修改的、供其它程序轮询检测的变量需要加volatile

// ---------------------- 直流电流测量 ----------------------
static volatile struct
{
    // 指定测量前延时时间
    int32_t time_delay;
    // 指定测量时间
    int32_t time_measurement;
    // 指定pwm占空比
    int32_t pwm_a;
    int32_t pwm_b;
    int32_t pwm_c;
    // 三相电流累加存储
    union {
        int32_t sum_a;
        float sumf_a2;
    };
    union {
        int32_t sum_b;
        float sumf_b2;
    };
    union {
        int32_t sum_c;
        float sumf_c2;
    };
    // 电源电压累加存储
    int32_t sum_vm;
    // 正弦波发生器峰值
    float sin_vp;
    // 正弦波发生器相位
    int32_t sin_count;
}calibration = {0};

// ---------------------- 温度 ----------------------
// 单片机温度，单位℃，在每次调用FOC_LowFrequencyTask时更新
static float temperature_mcu = 25.0f;
// MOS管温度，单位℃，在每次调用FOC_LowFrequencyTask时更新
static float temperature_ntc = 25.0f;

// ---------------------- 电压 ----------------------
// VREF电压，单位V，通过FOC_Set_Vref_VM_Voltage设置，后续不再更新
static float voltage_vref = 3.3f;
// 电机驱动电压，单位V，实时更新
static float voltage_vm = 24.0f;
// 根据VREF电压预先计算换算系数，节约时间
// VM电压换算系数，通过FOC_Set_Vref_VM_Voltage设置，后续不再更新
static float factor_vm = 0;
// 电压模式控制电压，相电压，单位V
static float voltage_open_loop_phase = 1.0f;
// 测量相电阻时使用的电压
// static float voltage_gain_calibration_phase = 2.0f;

// ---------------------- 电流 ----------------------
// 额定电流，有效值，单位A
static float current_rated = 3.5f;
// 峰值电流，有效值，单位A
//static float current_peak = 10.5f;
// 校准系数：三相电流采样的偏移值
static float current_offset_a = 0;
static float current_offset_b = 0;
static float current_offset_c = 0;
// 校准系数：通过FOC_Set_Vref_VM_Voltage设置，后续不再更新
static float current_Ka = 0;
// 校准系数：初始current_Ka = current_Kb，可校准，保存校准结果时存current_Kb/current_Ka的系数
static float current_Kb = 0;
// 校准系数：初始current_Ka = current_Kc，可校准，保存校准结果时存current_Kc/current_Ka的系数
static float current_Kc = 0;

// 校准后的三相电流测量值，单位A
static float current_a = 0;
static float current_b = 0;
static float current_c = 0;
// 动态零点校准，用来抵消运放温漂
static float current_dynamic_zero_fix = 0.0f;
// D、Q轴电流
static float current_id = 0;
static float current_iq = 0;

// ---------------------- 相电阻、相电感 ----------------------
static float phase_Rs = 0;
static float phase_Ls = 0;

// ---------------------- 角度 ----------------------
// 从角度传感器读取到的数据
static int32_t angle_original = 0;

// ---------------------- 工作模式 ----------------------
static volatile int32_t foc_mode = 0;

// 临时调试用的(TODO:删除，用不到)
// static float debug[4]={0};
// 临时调试用的(TODO:删除，用不到)
static volatile int32_t foc_counter = 0;

// 在开启PWM之前，必须调用FOC_Set_Vref_VM_Voltage
void FOC_Set_Vref_VM_Voltage(float vref, float vm){
    voltage_vref = vref;
    voltage_vm = vm;
    // 检查电压是否正常
    if(vm > PROTECT_VM_OVER_VOLTAGE){
        foc_mode = MODE_ERROR_VM_OVER_VOLTAGE;
    }else if(vm < PROTECT_VM_UNDER_VOLTAGE){
        foc_mode = MODE_ERROR_VM_UNDER_VOLTAGE;
    }
    if(vref > PROTECT_VREF_OVER_VOLTAGE){
        foc_mode = MODE_ERROR_VREF_OVER_VOLTAGE;
    }else if(vref < PROTECT_VREF_UNDER_VOLTAGE){
        foc_mode = MODE_ERROR_VREF_UNDER_VOLTAGE;
    }
    // 计算电流换算系数(初始参数)
    // 可以FOC_CAL_Current_Gain进一步校准， 也可以从FLASH加载校准参数
    current_Ka = vref / ( R_SENSE * OPAMP_GAIN * ADC_RESOLUTION );
    current_Kb = current_Ka;
    current_Kc = current_Ka;
    current_offset_a = -current_Ka * ADC_RESOLUTION / 2.0f;
    current_offset_b = current_offset_a;
    current_offset_c = current_offset_a;
    factor_vm = vref / (VM_FACTOR * ADC_RESOLUTION);
}

void FOC_Test_Mode(void){
    if(foc_mode >= 0){
        foc_mode = MODE_VOLATGE;
    }
}
// 将float转换为int32_t，如果溢出有饱和处理
CCMRAM static inline int32_t float_to_int32_with_clamp(float value) {
    int32_t result;
    __asm__ (
        "vcvt.s32.f32 %0, %1"
        : "=t"(result)
        : "t"(value)
    );
    return result;
}

// 输入input，单位为弧度，取值范围[-1, 1]
// 输出cos，取值范围[-1,1]
// 输出sin，取值范围[-1,1]
CCMRAM static inline void cordic_cos_sin(float input, float *cos, float *sin){
    // 将弧度转换为CORDIC角度格式（Q1.31格式）
    // CORDIC输入角度范围：[-1, 1 - 2^31] 对应 [-0x80000000, 0x7FFFFFFF]
    // C语言标准中，浮点数到整数的转换如果超出目标范围，是未定义行为，输出结果无法保证
    // 实测有饱和处理，但是更换编译器后情况可能会变化
    // int32_t angle_cordic = (int32_t)(input * 2147483648.0f);

    // 这里使用确保饱和处理的版本
    int32_t angle_cordic = float_to_int32_with_clamp(input * 2147483648.0f);

    // CORDIC的零开销模式
    // 当尝试读取CORDIC_RDATA寄存器时，如果计算没有完成，总线会自动插入等待周期，直到计算完成。因此，软件可以立即写入输入并立即读取结果，而无需轮询是否有效。
    LL_CORDIC_WriteData(CORDIC, angle_cordic);
    int32_t cos_q31 = (int32_t)LL_CORDIC_ReadData(CORDIC);
    int32_t sin_q31 = (int32_t)LL_CORDIC_ReadData(CORDIC);

    *cos = (float)cos_q31 / 2147483648.0f;  // 将Q1.31格式转换为浮点数[-1,1]
    *sin = (float)sin_q31 / 2147483648.0f;  // 除以2^31

    // printf_("input=%f, angle_cordic = %x, sin=%f, cos=%f\n", input, angle_cordic, *sin, *cos);
}

#if 0
// 测试三角函数精度与消耗时间
/*
    samples: 65536
    max sin error: 1.761597e-06
    max cos error: 1.668930e-06
    avg sin error: 3.815333e-07
    avg cos error: 3.835192e-07
    CORDIC total time: 18059 us
    Standard math total time: 162719 us
    Speed ratio: 9.01
 */
void verify_cordic_sin_cos(void)
{
    const int NUM_SAMPLES = 65536;
    float max_sin_error = 0.0f, max_cos_error = 0.0f;
    float avg_sin_error = 0.0f, avg_cos_error = 0.0f;

    for (int i = 0; i < NUM_SAMPLES; i++) {
        // 在[-1, 1]范围内均匀采样
        float input = -1.0f + 2.0f * i / (NUM_SAMPLES - 1);

        float cordic_sin, cordic_cos;
        cordic_cos_sin(input, &cordic_cos, &cordic_sin);
        // 计算标准数学库的结果
        float std_sin = sinf((float)M_PI * input);
        float std_cos = cosf((float)M_PI * input);
        // 计算误差
        float sin_error = fabsf(cordic_sin - std_sin);
        float cos_error = fabsf(cordic_cos - std_cos);

        if (sin_error > max_sin_error) max_sin_error = sin_error;
        if (cos_error > max_cos_error) max_cos_error = cos_error;

        avg_sin_error += sin_error;
        avg_cos_error += cos_error;

    }

    avg_sin_error /= NUM_SAMPLES;
    avg_cos_error /= NUM_SAMPLES;

    printf_("samples: %d\n", NUM_SAMPLES);
    printf_("max sin error: %e\n", max_sin_error);
    printf_("max cos error: %e\n", max_cos_error);
    printf_("avg sin error: %e\n", avg_sin_error);
    printf_("avg cos error: %e\n", avg_cos_error);

    // 测量CORDIC方案时间
    uint64_t start_time = get_absolute_time();
    for (int i = 0; i < NUM_SAMPLES; i++) {
        float input = -1.0f + 2.0f * i / (NUM_SAMPLES - 1);
        volatile float cordic_sin, cordic_cos;
        cordic_cos_sin(input, (float*)&cordic_cos, (float*)&cordic_sin);
    }
    uint64_t cordic_time = get_absolute_time() - start_time;

    // 测量标准数学库方案时间
    start_time = get_absolute_time();
    for (int i = 0; i < NUM_SAMPLES; i++) {
        float input = -1.0f + 2.0f * i / (NUM_SAMPLES - 1);
        volatile float std_sin = sinf((float)M_PI * input);
        volatile float std_cos = cosf((float)M_PI * input);
        (void)std_sin;
        (void)std_cos;
    }
    uint64_t std_time = get_absolute_time() - start_time;

    printf_("CORDIC total time: %llu us\n", cordic_time);
    printf_("Standard math total time: %llu us\n", std_time);
    printf_("Speed ratio: %.2f\n", (float)std_time / cordic_time);
}

// 单相死区补偿函数
// current 当前相电流值 last_dir 上一次电流方向（静态变量指针）pwm_duty 当前PWM占空比
// 低压下死区有非线性（MOS寄生二极管的导通压降远大于沟道本身），实测这个算法性能很差，不如不做补偿
// 可能比较适合开关慢，电压高的场合，例如IGBT
// 查看odrive的代码，也没有设计死区补偿

CCMRAM static inline int32_t dead_time_compensation(float current, int32_t* last_dir, int32_t pwm_duty) {
    // 死区补偿参数
    const int32_t DEAD_COMPENSATION = 11;  // 单边补偿量
    const float CURRENT_DEAD_ZONE = 0.02f;           // 电流死区阈值，避免在零点附近振荡
    const float HYSTERESIS_FACTOR = 1.2f;            // 滞回系数
    int32_t current_dir = 0;
    
    // 判断电流方向，考虑滞回效应
    if (current > CURRENT_DEAD_ZONE * HYSTERESIS_FACTOR || 
        (*last_dir > 0 && current > -CURRENT_DEAD_ZONE)) {
        current_dir = 1;  // 电流正向
    } else if (current < -CURRENT_DEAD_ZONE * HYSTERESIS_FACTOR || 
               (*last_dir < 0 && current < CURRENT_DEAD_ZONE)) {
        current_dir = -1; // 电流负向
    }
    
    // 根据电流方向进行补偿
    if (current_dir == 1) {
        // 电流正向：下桥臂寄生二极管导通，需要增加上桥臂占空比
        pwm_duty += DEAD_COMPENSATION;
    } else if (current_dir == -1) {
        // 电流负向：上桥臂寄生二极管导通，需要减少上桥臂占空比
        pwm_duty -= DEAD_COMPENSATION;
    }
    if (pwm_duty < 0){
        pwm_duty =  0;
    }  
    if (pwm_duty > PWM_MAX) {
        pwm_duty = PWM_MAX;
    }
    // 更新电流方向记录
    *last_dir = current_dir;
    
    return pwm_duty;
}
#endif

// Clark变换 a,b,c -> α,β
// α=a，只函数返回β，所有电流的单位都是A
CCMRAM static inline float clark(float b, float c){
    const float one_by_sqrt3 = 0.5773502691896258f;
    float beta = one_by_sqrt3 * (b - c);
    return beta;
}
// Park变换：alpha/beta -> Ud/Uq
// 所有电流的单位都是A，theta的取值范围是[-1,1]，对应[2,2*pi]
// Id ​= Iα​cos(θ)  + Iβ​sin(θ) 
// Iq ​= −Iα​sin(θ) + Iβ​cos(θ)
CCMRAM static inline void park(float alpha, float beta, float theta, float *id, float *iq){
    float cos_theta, sin_theta;
    cordic_cos_sin(theta, &cos_theta, &sin_theta);
    *id = alpha * cos_theta + beta * sin_theta;
    *iq = -alpha * sin_theta + beta * cos_theta;
}

// 逆Park变换：将Ud/Uq转换为alpha/beta，并且进行标幺化处理
// Vα = Vd*cos(θ) - Vq*sin(θ)
// Vβ = Vd*sin(θ) + Vq*cos(θ)
// Ud_input，Uq_input输入电压，单位V
// theta相位角，电角度，取值范围[-1,1]，对应[-pi, pi]
// Udc电源电压，单位V
CCMRAM static void inv_park(float Ud_input, float Uq_input, float theta, float Udc, float* alpha, float* beta) {
    // 标幺化处理（基准值 = 2/3*Udc, 最大相电压）
    // base表示在 SVPWM 控制下，逆变器能输出的​​最大相电压峰值​​
    // 当采用 SVPWM 调制时，最大不失真相电压幅值为直流母线电压Udc的1/sqrt(3)
    // 但算法中通过 2/3*Udc进行归一化，是因为该值对应​基本空间矢量的幅值​​（如矢量 100, 110 等的幅值）
    float base = 2.0f / 3.0f * Udc;
    float Ud = Ud_input / base;
    float Uq = Uq_input / base;

    float cos_theta, sin_theta;
    cordic_cos_sin(theta, &cos_theta, &sin_theta);

    *alpha = Ud * cos_theta - Uq * sin_theta;
    *beta  = Ud * sin_theta + Uq * cos_theta;
}

// SVPWM算法实现
// 参数: alpha, beta - αβ轴电压分量(标幺值，取值范围[-1, 1])
// 返回: U, V, W - 三相电压占空比(取值范围[0, 1])
// 当alpha^2 + beta^2 > ( sqrt(3)/2 )^2 时，输出波形会失真
// 参考 http://zhuanlan.zhihu.com/p/506240030
CCMRAM static void svpwm(float alpha, float beta, float* U, float* V, float* W) {
    // 常量定义
    const float sqrt3 = 1.73205080757f;  // sqrt(3)
    const float one_by_sqrt3 = 1.0f / sqrt3;
    const float two_by_sqrt3 = 2.0f / sqrt3;

    int sector;

    // 确定扇区
    if (beta >= 0.0f) {
        if (alpha >= 0.0f) {
            if (one_by_sqrt3 * beta > alpha) {
                sector = 2;
            } else {
                sector = 1;
            }
        } else {
            if (-one_by_sqrt3 * beta > alpha) {
                sector = 3;
            } else {
                sector = 2;
            }
        }
    } else {
        if (alpha >= 0.0f) {
            if (-one_by_sqrt3 * beta > alpha) {
                sector = 5;
            } else {
                sector = 6;
            }
        } else {
            if (one_by_sqrt3 * beta > alpha) {
                sector = 4;
            } else {
                sector = 5;
            }
        }
    }

    float t1, t2, t3, t4, t5, t6;
    float tA, tB, tC;

    // 根据扇区计算占空比
    switch (sector) {
        case 1:
            t1 = alpha - one_by_sqrt3 * beta;
            t2 = two_by_sqrt3 * beta;
            tA = (1.0f - t1 - t2) * 0.5f;
            tB = tA + t1;
            tC = tB + t2;
            break;

        case 2:
            t2 = alpha + one_by_sqrt3 * beta;
            t3 = -alpha + one_by_sqrt3 * beta;
            tB = (1.0f - t2 - t3) * 0.5f;
            tA = tB + t3;
            tC = tA + t2;
            break;

        case 3:
            t3 = two_by_sqrt3 * beta;
            t4 = -alpha - one_by_sqrt3 * beta;
            tB = (1.0f - t3 - t4) * 0.5f;
            tC = tB + t3;
            tA = tC + t4;
            break;

        case 4:
            t4 = -alpha + one_by_sqrt3 * beta;
            t5 = -two_by_sqrt3 * beta;
            tC = (1.0f - t4 - t5) * 0.5f;
            tB = tC + t5;
            tA = tB + t4;
            break;

        case 5:
            t5 = -alpha - one_by_sqrt3 * beta;
            t6 = alpha - one_by_sqrt3 * beta;
            tC = (1.0f - t5 - t6) * 0.5f;
            tA = tC + t5;
            tB = tA + t6;
            break;

        case 6:
            t6 = -two_by_sqrt3 * beta;
            t1 = alpha + one_by_sqrt3 * beta;
            tA = (1.0f - t6 - t1) * 0.5f;
            tC = tA + t1;
            tB = tC + t6;
            break;
    }

    // 确保占空比在[0,1]范围内
    *U = (tA < 0.0f) ? 0.0f : ((tA > 1.0f) ? 1.0f : tA);
    *V = (tB < 0.0f) ? 0.0f : ((tB > 1.0f) ? 1.0f : tB);
    *W = (tC < 0.0f) ? 0.0f : ((tC > 1.0f) ? 1.0f : tC);
}


void dump_debug(void){
    printf_("%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n", current_id, current_iq, current_a, current_b, current_c, (float)angle_original/ (float)0x1FFFFF);
    // if (debug_angle >= 0) {
    //     printf_("%.2f\n", debug_angle * (2 * PI / 0x1FFFFF));
    // }else{
    //     printf_("error code = %d\n", debug_angle);
    // }
}

/**
 * @brief 电流传感器增益校准与相电阻测量
 * 
 * 功能概述：
 * 1. 自动选择合适测试电压，使电流达到额定值
 * 2. 在6个不同方向施加电压矢量进行多点测量
 * 3. 使用最小二乘法校准B、C相电流传感器增益和偏移量
 * 4. 基于欧姆定律计算三相电阻
 * 5. 使用公式U = L*(di/dt) + I*R计算电感
 * 
 * 工作流程：
 * - 电压扫描：逐步增加电压至目标电流
 * - 数据采集：7组测量（1组零点+6组矢量）
 * - 增益校准：基于基尔霍夫电流定律进行最小二乘拟合
 * - 电阻计算：电压/电流法测量各相电阻
 * - 电感计算：测量绕组在施加1KHz正弦电压时的电流值，计算电感
 * 
 * 全局变量：
 * - 输入：current_rated, voltage_vm, current_Ka
 * - 输出：current_Kb, current_Kc, current_offset_a/b/c,
 *        phase_Rs, phase_Ls, 
 *        
 * - 状态：foc_mode
 * 
 * 可能发生的错误：
 * - 测试电流无法达到
 * - ADC量程饱和
 * - 电源电压不稳定
 * - 增益/电阻偏差超限
 */
int32_t FOC_Calibration(void){
    if(foc_mode < 0){
        return -1;
    }
    // ---------------- 挑选合适的测试电压 ----------------
    const float sqrt2 = 1.4142135623730951f;
    // 设定测试电流，相电流峰值，单位A
    float test_current = sqrt2 * current_rated;
    // 挑选测试电压，相电压峰值，例如2V
    float test_voltage = 0.0f;
    // 电源电压(仅用于设定PWM占空比，后续计算电阻，会使用实测电压vm_sum)
    float Udc = voltage_vm;

    while(1){
        // 逐步提升测试电压test_voltage，直到测试电流达到test_currnet
        test_voltage += 0.02f;
        // 相电压最大只能设置到电源电压的0.577倍，如果超过0.5倍电源电压，测试电流还达不到，报错
        if(test_voltage > 0.5f * Udc){
            printf_("FOC_CAL_Current_Gain Error: Can not reach test_current %.3fA\n", test_current);
            return -2;
        }
        // 开始校准
        float alpha, beta, a, b, c;
        inv_park(test_voltage, 0 , -1.0f, Udc, &alpha, &beta);
        svpwm(alpha, beta, &a, &b, &c);
        calibration.pwm_a = PWM_MAX * a;
        calibration.pwm_b = PWM_MAX * b;
        calibration.pwm_c = PWM_MAX * c;
        calibration.time_delay = 1;
        calibration.time_measurement = 64;
        foc_mode = MODE_MEASUREMENT_CURRENT_DELAY;
        // 等待采集数据
        while(foc_mode != MODE_MEASUREMENT_CURRENT_DONE);

        // 求平均
        float ca = calibration.sum_a / 64.0f;
        float cb = calibration.sum_b / 64.0f;
        float cc = calibration.sum_c / 64.0f;
        // 检查量程问题,应当在1-99%之间，不能出现ADC饱和的情况
        // 取值范围0 - ADC_RESOLUTION（4095）
        const int32_t cmin = ADC_RESOLUTION * 0.01f;
        const int32_t cmax = ADC_RESOLUTION * 0.99f;
        if(ca > cmax || ca < cmin || cb > cmax || cb < cmin || cc > cmax || cc < cmin){
            printf_("FOC_CAL_Current_Gain Error: Current out of range\n");
            return -3;
        }
        ca = fabsf(current_Ka * ca + current_offset_a);
        cb = fabsf(current_Kb * cb + current_offset_b);
        cc = fabsf(current_Kc * cc + current_offset_c);
        if(ca > test_current || cb > test_current/2.0f || cc > test_current/2.0f){
            break;
        }
    }
    printf_("select test voltage %.3fV for test current %.3fA\n", test_voltage, test_current);

    // ---------------- 测量部分 ----------------
    // 生成测试数据，其中第一组用于测试零点，其他的用于测量增益误差和电阻
    // Test 0: va=0.000V, vb=0.000V, vc=0.000V, ia=2081, ib=2035, ic=2062
    int32_t test_pwm[7][3] = {
        {PWM_MAX / 2, PWM_MAX / 2, PWM_MAX / 2},
    };
    // 其他组挑选每一相电压都不为0的数据
    // Test 1: va=2.004V, vb=-1.002V, vc=-1.002V, ia=1649, ib=2255, ic=2282
    // Test 2: va=1.002V, vb=1.002V, vc=-2.004V, ia=1864, ib=1818, ic=2494
    // Test 3: va=-1.002V, vb=2.004V, vc=-1.002V, ia=2297, ib=1598, ic=2275
    // Test 4: va=-2.004V, vb=1.002V, vc=1.002V, ia=2515, ib=1815, ic=1847
    // Test 5: va=-1.002V, vb=-1.002V, vc=2.004V, ia=2301, ib=2252, ic=1636
    // Test 6: va=1.002V, vb=-2.004V, vc=1.002V, ia=1869, ib=2471, ic=1854
    for(int i=0; i<6; i++){
        float alpha, beta, a, b, c;
        float theta = (i - 3) / 3.0f;
        inv_park(test_voltage, 0 , theta, Udc, &alpha, &beta);
        svpwm(alpha, beta, &a, &b, &c);
        test_pwm[i+1][0] = PWM_MAX * a;
        test_pwm[i+1][1] = PWM_MAX * b;
        test_pwm[i+1][2] = PWM_MAX * c;
    }
 
    float current[7][3]; // ADC采样原始值
    float voltage[7];    // 换算后的电压，单位V
    for(int i=0; i<7; i++){
        // 开始校准
        calibration.pwm_a = test_pwm[i][0];
        calibration.pwm_b = test_pwm[i][1];
        calibration.pwm_c = test_pwm[i][2];
        calibration.time_delay = 8192; // 273ms
        calibration.time_measurement = 2048;
        foc_mode = MODE_MEASUREMENT_CURRENT_DELAY;

        // 等待采集数据
        while(foc_mode != MODE_MEASUREMENT_CURRENT_DONE);

        // 求平均
        current[i][0] = calibration.sum_a / 2048.0f;;
        current[i][1] = calibration.sum_b / 2048.0f;
        current[i][2] = calibration.sum_c / 2048.0f;
        // 检查量程问题,应当在1-99%之间，不能出现ADC饱和的情况
        // 取值范围0 - ADC_RESOLUTION（4095）
        const int32_t cmin = ADC_RESOLUTION * 0.01f;
        const int32_t cmax = ADC_RESOLUTION * 0.99f;
        if(current[i][0] > cmax || current[i][0] < cmin || 
            current[i][1] > cmax || current[i][1] < cmin || 
            current[i][2] > cmax || current[i][2] < cmin){
            printf_("FOC_CAL_Current_Gain Error: Current out of range\n");
            return -4;
        }
        voltage[i] = calibration.sum_vm * factor_vm / 2048.0f;
        // 如果测试过程电压波动超过5%，报错
        if(fabsf((voltage[i] - Udc) / Udc) > 0.05f){
            printf_("FOC_CAL_Current_Gain Error: Power voltage unstable\n");
            return -5;
        }
        // printf_("%f %f %f %f\n", voltage[i],current[i][0],current[i][1],current[i][2]);
    }

    //  ---------------- 计算电流放大器误差系数 ----------------
    // 求Kb, Kc, offset_b, offset_c
    // 使用第一组数据（零点），推断offset_a， offset_b， offset_c与Ka、Kb、Kc的关系
    float Ka = current_Ka;
    float Kb, Kc;

    // 对于i = 1,2,3,4,5,6
    // Ka * (current[i][0] - current[0][0]) + Kb * (current[i][1] - current[0][1]) + Kc * (current[i][2] - current[0][2]) = zero[i]
    // 使用最小二乘法求Kb, Kc，使zero[i]的值尽可能接近0，并且输出最终的六组zero值
    
    // 构建最小二乘法矩阵
    float sum_x1x1 = 0, sum_x1x2 = 0, sum_x2x2 = 0;
    float sum_x1y = 0, sum_x2y = 0;
    
    for(int i = 1; i < 7; i++) {
        float x1 = current[i][1] - current[0][1];  // B相电流变化量
        float x2 = current[i][2] - current[0][2];  // C相电流变化量
        float y = -Ka * (current[i][0] - current[0][0]);  // -Ka * A相电流变化量
        
        sum_x1x1 += x1 * x1;
        sum_x1x2 += x1 * x2;
        sum_x2x2 += x2 * x2;
        sum_x1y += x1 * y;
        sum_x2y += x2 * y;
    }
    
    // 计算行列式
    float det = sum_x1x1 * sum_x2x2 - sum_x1x2 * sum_x1x2;
    
    if(fabsf(det) < 1e-6) {
        printf_("FOC_CAL_Current_Gain Error: Matrix is singular\n");
        return -6;
    }
    
    // 求解Kb, Kc
    Kb = (sum_x2x2 * sum_x1y - sum_x1x2 * sum_x2y) / det;
    Kc = (sum_x1x1 * sum_x2y - sum_x1x2 * sum_x1y) / det;
    
    // 计算偏移量
    float offset_a = -Ka * current[0][0];
    float offset_b = -Kb * current[0][1];
    float offset_c = -Kc * current[0][2];
    
    printf_("Calibration results: Ka=%.6f, Kb=%.6f, Kc=%.6f, offset_a=%.3f, offset_b=%.3f, offset_c=%.3f\n",
           Ka, Kb, Kc, offset_a, offset_b, offset_c);

    // 检查数据是否正常, 偏差不超过PROTECT_CURRENT_SENSOR
    float avg_k = (Ka + Kb + Kc) / 3.0f;
    if(fabsf(Kb - avg_k)/avg_k > PROTECT_CURRENT_SENSOR || fabsf(Kc - avg_k)/avg_k > PROTECT_CURRENT_SENSOR){
        printf_("FOC_CAL_Current_Gain Error: Too large deviation\n");
        return -7;
    }
    float offset_avg = (offset_a + offset_b + offset_c) / 3.0f;
    if(fabsf((offset_b - offset_avg) / offset_avg) > PROTECT_CURRENT_SENSOR || 
       fabsf((offset_c - offset_avg) / offset_avg) > PROTECT_CURRENT_SENSOR ){
        printf_("FOC_CAL_Current_Gain Error: Offset deviation too large\n");
        return -8;
    }

    // 保存计算结果
    current_offset_a = offset_a;
    current_offset_b = offset_b;
    current_offset_c = offset_c;
    current_Kb = Kb;
    current_Kc = Kc;

    // ---------------- 分别计算三相电阻的代码 ----------------
    float Ra = 0, Rb = 0, Rc = 0;
    for(int i=1; i<7; i++){
        // 计算相电流(校准后的)
        float Ia = Ka * current[i][0] + offset_a;
        float Ib = Kb * current[i][1] + offset_b;
        float Ic = Kc * current[i][2] + offset_c;
        // 计算相电压
        float Va = voltage[i] / PWM_MAX * test_pwm[i][0];
        float Vb = voltage[i] / PWM_MAX * test_pwm[i][1];
        float Vc = voltage[i] / PWM_MAX * test_pwm[i][2];
        float avg = (Va + Vb + Vc) / 3;
        Va -= avg;
        Vb -= avg;
        Vc -= avg;
        printf_("Test %d: Va=%.3fV, Vb=%.3fV, Vc=%.3fV, Ia=%.3fA, Ib=%.3fA, Ic=%.3fA\n", i, Va, Vb, Vc, Ia, Ib, Ic);
        Ra += fabsf(Va / Ia);
        Rb += fabsf(Vb / Ib);
        Rc += fabsf(Vc / Ic);
    }
    Ra /= 6.0f;
    Rb /= 6.0f;
    Rc /= 6.0f;

    // 打印测试结果
    printf_("Phase resistances: Ra = %.3f ohm, Rb = %.3f ohm, Rc = %.3f ohm\n", Ra, Rb, Rc);
    // ---------------- 检查三相电阻偏差 ----------------
    // 计算三相电阻的平均值
    float R_avg = (Ra + Rb + Rc) / 3.0f;
    
    // 计算各相电阻的相对偏差
    float Ra_deviation = fabsf(Ra - R_avg) / R_avg;
    float Rb_deviation = fabsf(Rb - R_avg) / R_avg;
    float Rc_deviation = fabsf(Rc - R_avg) / R_avg;
    
    // 检查是否有任何一相的偏差超过阈值
    if(Ra_deviation > PROTECT_CURRENT_SENSOR || 
       Rb_deviation > PROTECT_CURRENT_SENSOR || 
       Rc_deviation > PROTECT_CURRENT_SENSOR ) {
        printf_("FOC_CAL_Current_Gain Error: Phase resistance deviation is too large\n");
        return -9;
    }
    // 保存计算结果
    phase_Rs = R_avg;

    // ---------------- 测量相电感 ----------------
    // 电机的相电感是非线性的，不同电流大小、电流频率、转子位置测量，会有一定的差异
    // 此处使用1KHz的正弦波进行测试，以下是S42BL61电机的实测值
    // voltage(RMS) = 2.220V, currnet(RMS) = 0.625A, L = 0.491 mH
    // voltage(RMS) = 6.826V, currnet(RMS) = 1.835A, L = 0.518 mH
    //                    +------- Lb ------ 0V
    // sin_vp ----- La ---+------- Lc ------ -sin_vp

    // 测试时的电压有效值为2*sin_vp/sqrt(2),
    // 测试时电感为2*L,电阻是2*R_avg
    float L_avg = 0.0f;
    for(int i=0; i<2; i++){
        // 1000Hz测试频率，如需修改，中断中的正弦波相关的代码也要改动
        const float FREQ = 1000.0f;
        float rms_voltage;
        if(i == 0) {
            // 相电感La两端的电压，第一次取保守的值，也就是电流传感器校准时的电压
            rms_voltage = test_voltage; 
        }else{
            // 第二次测试，根据初测的电感值，加大测试电流，取得更接近实际工作状态的相电感
            // 无刷电机的电感值是非线性的，会随电流变化
            rms_voltage = test_current * (R_avg + 2.0f*PI*FREQ*L_avg); 
        }
        float max_rms_voltage = Udc / 2.0f / sqrt2 * 0.8f;
        if(rms_voltage > max_rms_voltage){
            rms_voltage = max_rms_voltage;
        }
        calibration.time_delay = 3000; 
        calibration.time_measurement = 3000; // 包含整数个（3000/30=100个）正弦周期
        calibration.sin_vp = rms_voltage * sqrt2;
        foc_mode = MODE_MEASUREMENT_INDUCTANCE_DELAY;
        // 等待采集数据
        while(foc_mode != MODE_MEASUREMENT_INDUCTANCE_DONE);
        // 求平均
        float ca = __builtin_sqrtf(calibration.sumf_a2 / 3000.0f);
        float cc = __builtin_sqrtf(calibration.sumf_c2 / 3000.0f);
        // 理论上ca = cb
        float rms_current = (ca + cc) / 2.0f;
        // 计算电感rms_voltage / rms_current = R + 2*pi*f*L
        L_avg = (rms_voltage / rms_current - R_avg) / (2.0f*PI*FREQ);
        printf_("voltage(RMS) = %.3fV, currnet(RMS) = %.3fA, L = %.3f mH\n", rms_voltage, rms_current, L_avg*1000.0f);
        if(i != 0){
            phase_Ls = L_avg;
        }
    }
    foc_mode = MODE_IDLE;
    return 0;
}

// 电流环任务，每秒30K次，需要在下次TIM1更新事件(计数器向下溢出)之前完成
// 开始读取磁编码器 --> ADC采样开始 --> 计数器向上溢出 --> ADC采样完成（同时完成磁编码器读取） --> 执行FOC_HighFrequencyTask --> 计数器向下溢出
//      -5us     -->   -0.5us   -->     T0=0us   -->             0.5us             -->                           -->  16.7us
CCMRAM void FOC_HighFrequencyTask(void){
    // 输出的PWM占空比，0 - PWM_MAX（PWM_MAX = 2500）
    // static类型，保留至下一次调用FOC_HighFrequencyTask备用，拿来判断电流采样窗口
    static int32_t pwm_a = 0, pwm_b = 0, pwm_c = 0;
    // 角度传感器错误计数，当连续出现PROTECT_MT6835_ERROR_COUNT次通信异常，将进入故障保护状态
    static int32_t angle_mt6835_error_count = 0;
    // ------------------- 获取电流电压数据 -------------------
    // 读取ADC1注入组数据
    uint16_t current_adc_a = LL_ADC_INJ_ReadConversionData12(ADC1, LL_ADC_INJ_RANK_1);  // ADC1_IN3
    uint16_t current_adc_c = LL_ADC_INJ_ReadConversionData12(ADC1, LL_ADC_INJ_RANK_2);  // ADC1_IN12
    // 读取ADC2注入组数据
    uint16_t vm_adc   = LL_ADC_INJ_ReadConversionData12(ADC2, LL_ADC_INJ_RANK_1);  // ADC2_IN1
    uint16_t current_adc_b = LL_ADC_INJ_ReadConversionData12(ADC2, LL_ADC_INJ_RANK_2);  // ADC2_IN3
    // ------------------- 获取角度数据 -------------------
    // 读取传感器角度数据
    int32_t Angle_MT6835 = MT6835_Finish_DMA_Read_Angle();
    // 判断角度传感器有无故障
    if(Angle_MT6835 < 0){
        if(foc_mode >= 0 && ++angle_mt6835_error_count >= PROTECT_MT6835_ERROR_COUNT){
            foc_mode = MODE_ERROR_MT6835;
        }
    }else{
        angle_mt6835_error_count = 0;
        angle_original = Angle_MT6835;
    }
    // ------------------- 校准电流电压数据 -------------------
    // 电源电压平滑滤波(截止频率1KHz)
    const float voltage_filter_factor = 2 * PI * 1000 / 30000;
    float vm_raw = vm_adc * factor_vm;
    voltage_vm += (vm_raw - voltage_vm) * voltage_filter_factor;
    // 检查电压是否正常
    if(voltage_vm > PROTECT_VM_OVER_VOLTAGE){
        foc_mode = MODE_ERROR_VM_OVER_VOLTAGE;
    }else if(voltage_vm < PROTECT_VM_UNDER_VOLTAGE){
        foc_mode = MODE_ERROR_VM_UNDER_VOLTAGE;
    }
    // 丢弃采样窗口过小的相，计算三相电流
    // 最难采集的情况的PWM如下，在SVPWM峰值100%时达到
    // a: （2 - sqrt(3)) / 4 = 0.0669872981077807，对应PWM寄存器167
    // b: （2 + sqrt(3)) / 4 = 0.9330127018922193，对应PWM寄存器2333，有2.2us的电流采样窗口
    // c: （2 + sqrt(3)) / 4 = 0.9330127018922193，对应PWM寄存器2333，有2.2us的电流采样窗口
    // 找出占空比最大的一相
    int32_t max_pwm = pwm_a;
    char max_phase = 'a';
    if (pwm_b > max_pwm) {
        max_pwm = pwm_b;
        max_phase = 'b';
    }
    if (pwm_c > max_pwm) {
        max_pwm = pwm_c;
        max_phase = 'c';
    }
    if (max_pwm >= PWM_MAX * 0.8f) {
        // 丢弃pwm_a、pwm_b、pwm_c中占空比最大的一相，使用a+b+c=0重新计算
        switch (max_phase) {
            case 'a': // 丢弃A相
                current_b = current_Kb * current_adc_b + current_offset_b;
                current_c = current_Kc * current_adc_c + current_offset_c;
                current_a = - (current_b + current_c);
                break;
            case 'b': // 丢弃B相
                current_a = current_Ka * current_adc_a + current_offset_a;
                current_c = current_Kc * current_adc_c + current_offset_c;
                current_b = - (current_a + current_c);
                break;
            case 'c': // 丢弃C相
                current_a = current_Ka * current_adc_a + current_offset_a;
                current_b = current_Kb * current_adc_b + current_offset_b;
                current_c = - (current_a + current_b);
                break;
        }
    }else{
        // 如果pwm_a pwm_b pwm_c均小于80%，正常计算电流值
        current_a = current_Ka * current_adc_a + current_offset_a;
        current_b = current_Kb * current_adc_b + current_offset_b;
        current_c = current_Kc * current_adc_c + current_offset_c;

        // 受限于电流采样电路的性能，零点有轻微漂移，采用动态电流零点校准方式处理
        // 动态电流零点校准，在pwm_a pwm_b pwm_c均小于80%时才触发
        const float zero_filter_factor = 2 * PI * 1.0f / 30000;
        float zero_point = (current_a + current_b + current_c) / 3.0f;
        current_dynamic_zero_fix += (zero_point - current_dynamic_zero_fix) * zero_filter_factor;
        current_a -= current_dynamic_zero_fix;
        current_b -= current_dynamic_zero_fix;
        current_c -= current_dynamic_zero_fix;
    }
    if(fabsf(current_a) > PROTECT_OVER_CURRENT || 
       fabsf(current_b) > PROTECT_OVER_CURRENT || 
       fabsf(current_c) > PROTECT_OVER_CURRENT){
        foc_mode = MODE_ERROR_OVER_CURRENT;
    }
    // ------------------- 电流环 -------------------
    int32_t foc_mode_copy = foc_mode;
    // 检查驱动器状态
    static_assert(READY_GPIO_Port == NFAULT_GPIO_Port);
    if(foc_mode_copy != MODE_IDLE){
        uint32_t port_stat = LL_GPIO_ReadInputPort(READY_GPIO_Port);
        if((port_stat & READY_Pin) == 0 || (port_stat & NFAULT_Pin) == 0){
            foc_mode = foc_mode_copy = MODE_ERROR_DRIVER;
        }
    }
    // 出现错误时，立即停止PWM输出
    if(foc_mode_copy < 0){
        LL_TIM_DisableCounter(TIM1);
        LL_TIM_DisableAllOutputs(TIM1);
        // 强制输出无效电平（根据配置，OCIdleState为低电平）
        // 这样可以确保在定时器停止时所有输出都为低电平
        LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH1);
        LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH2);
        LL_TIM_OC_DisableFast(TIM1, LL_TIM_CHANNEL_CH3);
        LL_ADC_DisableIT_JEOS(ADC1);
        LL_TIM_DisableIT_CC1(TIM3);
        return;
    }
    switch(foc_mode_copy){
        case MODE_IDLE:
            pwm_a = 0;
            pwm_b = 0;
            pwm_c = 0;
            break;
        // 测量电流传感器增益偏移 + 电阻
        case MODE_MEASUREMENT_CURRENT_DELAY: 
            pwm_a = calibration.pwm_a;
            pwm_b = calibration.pwm_b;
            pwm_c = calibration.pwm_c;
            calibration.sum_a = 0;
            calibration.sum_b = 0;
            calibration.sum_c = 0;
            calibration.sum_vm = 0;
            if(--calibration.time_delay == 0){
                foc_mode = MODE_MEASUREMENT_CURRENT_RUN;
            }
            break;
        case MODE_MEASUREMENT_CURRENT_RUN:
            calibration.sum_a += current_adc_a;
            calibration.sum_b += current_adc_b;
            calibration.sum_c += current_adc_c;
            calibration.sum_vm += vm_adc;
            if(--calibration.time_measurement == 0){
                foc_mode = MODE_MEASUREMENT_CURRENT_DONE;
            }
            break;
        case MODE_MEASUREMENT_CURRENT_DONE:
            // 保持PWM占空比不变
            break;
        // 测量电感(1KHz测试电压下的电流有效值)
        // 30KHz / 1KHz = 30, 30/2 = 15
        case MODE_MEASUREMENT_INDUCTANCE_DELAY: 
        {
            float cos_t, sin_t;
            if(++calibration.sin_count == 30) calibration.sin_count = 0;
            cordic_cos_sin((calibration.sin_count - 15) / 15.0f, &cos_t, &sin_t);
            int32_t v = (int32_t)( sin_t * calibration.sin_vp * PWM_MAX / voltage_vm);
            pwm_a = PWM_MAX / 2 + v;
            pwm_b = PWM_MAX / 2;
            pwm_c = PWM_MAX / 2 - v;
            calibration.sumf_a2 = 0.0f;
            calibration.sumf_b2 = 0.0f;
            calibration.sumf_c2 = 0.0f;
            calibration.sum_vm = 0;
            if(--calibration.time_delay == 0){
                foc_mode = MODE_MEASUREMENT_INDUCTANCE_RUN;
            }
        }
        break;
        case MODE_MEASUREMENT_INDUCTANCE_RUN:
        {
            float cos_t, sin_t;
            if(++calibration.sin_count == 30) calibration.sin_count = 0;
            cordic_cos_sin((calibration.sin_count - 15) / 15.0f, &cos_t, &sin_t);
            int32_t v = (int32_t)( sin_t * calibration.sin_vp * PWM_MAX / voltage_vm);
            pwm_a = PWM_MAX / 2 + v;
            pwm_b = PWM_MAX / 2;
            pwm_c = PWM_MAX / 2 - v;
            calibration.sumf_a2 += current_a * current_a;
            calibration.sumf_b2 += current_b * current_b;
            calibration.sumf_c2 += current_c * current_c;
            calibration.sum_vm += vm_adc;
            if(--calibration.time_measurement == 0){
                foc_mode = MODE_MEASUREMENT_INDUCTANCE_DONE;
            }
            break;
        }
        case MODE_MEASUREMENT_INDUCTANCE_DONE:
            pwm_a = PWM_MAX / 2;
            pwm_b = PWM_MAX / 2;
            pwm_c = PWM_MAX / 2;
            break;
        case MODE_VOLATGE:
            // 处理数据
            if(foc_mode){
                if(++foc_counter == 100000){
                    foc_counter = 0;
                }
                float theta = (foc_counter - 50000)/50000.0f;

                const float Udc = 24.0f;
                const float Uq = 0.0f;
                const float Ud = 1.0f;
                float voltage_a, voltage_b, voltage_c;
                float alpha, beta;
               
                inv_park(Ud, Uq, theta, Udc, &alpha, &beta);
                svpwm(alpha, beta, &voltage_a, &voltage_b, &voltage_c);
                pwm_a = PWM_MAX * voltage_a;
                pwm_b = PWM_MAX * voltage_b;
                pwm_c = PWM_MAX * voltage_c;


                // 测试高PWM占空比（93%）时的电流采样
                // pwm_a += 993;
                // pwm_b += 993;
                // pwm_c += 993;

                // 计算Id， Iq
                float current_beta = clark(current_b, current_c);
                park(current_a, current_beta, theta, &current_id, &current_iq);
            }
            break;
        default:
            break;
    }
    
    LL_TIM_OC_SetCompareCH1(TIM1, pwm_a);
    LL_TIM_OC_SetCompareCH2(TIM1, pwm_b);
    LL_TIM_OC_SetCompareCH3(TIM1, pwm_c);
}

// 速度环、位置环任务，每秒10K次
void FOC_MediumFrequencyTask(void){
    // LED2_TOGGLE();
}

// 过热保护，LED状态指示等，利用空闲时间调用
// 50ms调用一次
void FOC_LowFrequencyTask(void){
    static int led_state = 0;        // LED状态机状态
    static int blink_count = 0;      // 闪烁计数
    static int pause_count = 0;      // 暂停计数
    static int pattern_index = 0;    // 当前闪烁模式位索引
    
    get_temperature(&temperature_mcu, &temperature_ntc, voltage_vref);
    if(temperature_mcu > PROTECT_MCU_OVER_TEMPERATURE || temperature_ntc > PROTECT_MCU_OVER_TEMPERATURE){
        foc_mode = MODE_ERROR_OVER_TEMPERATURE;
    }

    if(foc_mode >= 0){
        dump_debug();
    }else{
        // printf_("error %d\n", foc_mode);
        // 使用状态机方式完成LED故障指示
        int pattern = -foc_mode; // 转换为正数(1~15)

        switch(led_state){
            case 0: // 闪烁序列状态
                if(pattern_index < 4){ // 处理4位闪烁模式
                    int bit_value = (pattern >> (3 - pattern_index)) & 0x01; // 从高位到低位读取
                    int blink_duration = (bit_value == 0) ? 2 : 8; // 短:100ms(2 * 50ms), 长:400ms(8 * 50ms)
                    
                    if(blink_count < blink_duration){
                        LED2_ON(); // 点亮阶段
                        blink_count++;
                    }
                    else if(blink_count < blink_duration + 6){ // 熄灭阶段
                        LED2_OFF(); // 熄灭阶段
                        blink_count++;
                    }
                    else{
                        // 当前位闪烁完成，进入下一位
                        blink_count = 0;
                        pattern_index++;
                    }
                }
                else{
                    // 4位闪烁完成，进入暂停状态
                    pattern_index = 0;
                    blink_count = 0;
                    pause_count = 0;
                    led_state = 1;
                    LED2_OFF();
                }
                break;
                
            case 1: // 暂停状态(2秒)
                LED2_OFF();
                if(pause_count < 40){ // 2000ms = 40 * 50ms
                    pause_count++;
                }
                else{
                    // 暂停完成，重新开始闪烁序列
                    pause_count = 0;
                    led_state = 0;
                }
                break;
                
            default:
                led_state = 0;
                break;
        }
    }
}